#ifndef RAY_GROUND_FILTER_HPP_
#define RAY_GROUND_FILTER_HPP_

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Header.h>

#include <tf/transform_listener.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_eigen/tf2_eigen.h>

#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/optional.hpp>

#include <Eigen/Dense>
#include <memory>
#include <string>
#include <vector>

namespace bg = boost::geometry;
using Point = bg::model::d2::point_xy<double>;
using Polygon = bg::model::polygon<Point>;

namespace autoware_ground_segmentation
{

class RayGroundFilter
{
  typedef pcl::PointXYZ PointType_;

  struct PointXYZRTColor
  {
    pcl::PointXYZ point;
    size_t ring;  // ring number if available
    float radius;  // cylindrical coords on XY Plane
    float theta;   // angle deg on XY plane
    size_t radial_div;  // index of the radial division to which this point belongs to
    size_t red;    // Red component  [0-255]
    size_t green;  // Green Component[0-255]
    size_t blue;   // Blue component [0-255]
    size_t original_index;  // index of this point in the source pointcloud
  };
  typedef std::vector<PointXYZRTColor> PointCloudXYZRTColor;

public:
  RayGroundFilter(ros::NodeHandle& nh, ros::NodeHandle& private_nh);
  ~RayGroundFilter() = default;

private:
  void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& input_msg);
  
  void ConvertXYZIToRTZColor(
    const pcl::PointCloud<PointType_>::Ptr in_cloud, 
    PointCloudXYZRTColor & out_organized_points,
    std::vector<pcl::PointIndices> & out_radial_divided_indices,
    std::vector<PointCloudXYZRTColor> & out_radial_ordered_clouds);

  void ClassifyPointCloud(
    std::vector<PointCloudXYZRTColor> & in_radial_ordered_clouds,
    pcl::PointIndices & out_ground_indices, 
    pcl::PointIndices & out_no_ground_indices);

  void ExtractPointsIndices(
    const sensor_msgs::PointCloud2::ConstPtr in_cloud_ptr, 
    const pcl::PointIndices & in_indices,
    sensor_msgs::PointCloud2::Ptr ground_cloud_msg_ptr, 
    sensor_msgs::PointCloud2::Ptr no_ground_cloud_msg_ptr);

  boost::optional<float> calcPointVehicleIntersection(const Point & point);
  void setVehicleFootprint(const double min_x, const double max_x, const double min_y, const double max_y);
  void initializePointCloud2(const sensor_msgs::PointCloud2::ConstPtr & in_cloud_ptr,
                            const sensor_msgs::PointCloud2::Ptr & out_cloud_msg_ptr);

  // ROS related
  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;
  ros::Subscriber pointcloud_sub_;
  ros::Publisher ground_cloud_pub_;
  ros::Publisher no_ground_cloud_pub_;

  // TF related
  tf::TransformListener tf_listener_;
  tf2_ros::Buffer tf2_buffer_;
  tf2_ros::TransformListener tf2_listener_;

  // Parameters
  double general_max_slope_;            // degrees
  double local_max_slope_;              // degrees
  double initial_max_slope_;            // degrees
  double radial_divider_angle_;         // distance in rads between dividers
  double concentric_divider_distance_;  // distance in meters between concentric divisions
  double min_height_threshold_;         // minimum height threshold regardless the slope
  double reclass_distance_threshold_;   // distance between points at which re classification will occur

  size_t radial_dividers_num_;
  size_t grid_width_;
  size_t grid_height_;
  double grid_precision_;

  double min_x_;
  double max_x_;
  double min_y_;
  double max_y_;

  Polygon vehicle_footprint_;
  bool use_vehicle_footprint_;

  std::vector<cv::Scalar> colors_;
  const size_t color_num_ = 10;  // different number of color to generate
  pcl::PointCloud<PointType_>::Ptr previous_cloud_ptr_;  // holds the previous groundless result

  // Point cloud processing
  pcl::VoxelGrid<PointType_> voxel_grid_;
  pcl::ExtractIndices<PointType_> extract_indices_;
};

} // namespace autoware_ground_segmentation

#endif // RAY_GROUND_FILTER_HPP_ 